//------------------------------------------------------------------------------
//  <copyright file="CSMDGenerators.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "stdafx.h"
#include "CSMDGenerators.h"

#include "ControllerStateMachineExports.h"
#include <ControllerStateMachineDescription.cs>
#include <KukaLBR4Definitions.h>
#include <KukaLBR4Operations.h>

/// <summary>
/// Generate a csmd that waits for the FRI state to enter CMD mode and then execute a position movement 
/// </summary>
/// <returns>
/// A <c>ControllerStateMachineDescription</c> for a csmd that waits for the FRI state
/// to enter CMD mode and then executes a position movement.
/// </returns>
KUKALBR4NATIVETESTS_API ControllerStateMachineDescription GenerateSimplePositionCsmd()
{
	ControllerStateMachineDescription csmd;

	csmd.NumStates = 2;

	csmd.State0.ControlMode = ControlModes::NoneControlMode;
	csmd.State0.Operations.NumOperations = 1;
	csmd.State0.NumTransitions = 1;

	csmd.State0.Operations.Operation0.OpCode = KukaLBR4Operations::CopyVec7Op;
	csmd.State0.Operations.Operation0.NumOperands = 1;
	csmd.State0.Operations.Operation0.Result.RegisterNumber = KukaLBR4PlusWorkingRegisters::JointVec1Register;
	csmd.State0.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;
	csmd.State0.Operations.Operation0.Operand0.RegisterNumber = KukaLBR4PlusFeedbackRegisters::LastIssuedJointPositionCommandRegister;
	csmd.State0.Operations.Operation0.Operand0.RegisterType = RegisterTypes::Feedback;

	csmd.State0.Transition0.NumTestOperands = 2;
	csmd.State0.Transition0.TestOpCode = KukaLBR4Operations::AreScalarsEqualOp;
	csmd.State0.Transition0.TestOperand0.RegisterNumber = KukaLBR4PlusFeedbackRegisters::FRIStateRegister;
	csmd.State0.Transition0.TestOperand0.RegisterType = RegisterTypes::Feedback;
	csmd.State0.Transition0.TestOperand1.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar1Register;
	csmd.State0.Transition0.TestOperand1.RegisterType = RegisterTypes::Working;
	csmd.State0.Transition0.TargetState = 1;

	csmd.State1.ControlMode = ControlModes::JointPositionControlMode;
	csmd.State1.NumTransitions = 0;
	csmd.State1.Operations.NumOperations = 2;

	csmd.State1.Operations.Operation0.OpCode = KukaLBR4Operations::AddOp;
	csmd.State1.Operations.Operation0.NumOperands = 2;
	csmd.State1.Operations.Operation0.Result.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar6Register;
	csmd.State1.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation0.Operand0.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar5Register;
	csmd.State1.Operations.Operation0.Operand0.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation0.Operand1.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar6Register;
	csmd.State1.Operations.Operation0.Operand1.RegisterType = RegisterTypes::Working;

	csmd.State1.Operations.Operation1.OpCode = KukaLBR4Operations::SimpleJointInterpolationOp;
	csmd.State1.Operations.Operation1.NumOperands = 6;
	csmd.State1.Operations.Operation1.Result.RegisterNumber = KukaLBR4PlusWorkingRegisters::JointPositionCommandRegister;
	csmd.State1.Operations.Operation1.Result.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand0.RegisterNumber = KukaLBR4PlusWorkingRegisters::JointVec1Register;
	csmd.State1.Operations.Operation1.Operand0.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand1.RegisterNumber = KukaLBR4PlusWorkingRegisters::JointVec2Register;
	csmd.State1.Operations.Operation1.Operand1.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand2.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar2Register;
	csmd.State1.Operations.Operation1.Operand2.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand3.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar3Register;
	csmd.State1.Operations.Operation1.Operand3.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand4.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar4Register;
	csmd.State1.Operations.Operation1.Operand4.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand5.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar6Register;
	csmd.State1.Operations.Operation1.Operand5.RegisterType = RegisterTypes::Working;

	return csmd;
}

/// <summary>
/// Generate a csmd that waits for the FRI state to enter CMD mode and then execute a Cartesian position movement 
/// </summary>
/// <returns>
/// A <c>ControllerStateMachineDescription</c> for a csmd that waits for the FRI state
/// to enter CMD mode and then executes a position movement.
/// </returns>
KUKALBR4NATIVETESTS_API ControllerStateMachineDescription GenerateSimpleCartesianDeltaMoveCsmd()
{
	ControllerStateMachineDescription csmd;

	csmd.NumStates = 2;

	csmd.State0.ControlMode = ControlModes::CartesianImpedanceControlMode;
	csmd.State0.Operations.NumOperations = 1;
	csmd.State0.NumTransitions = 1;
	
	// Make sure our current commanded position is our actual position.  Prevents problems switching to command mode
	csmd.State0.Operations.Operation0.OpCode = KukaLBR4Operations::CopyFrameOp;
	csmd.State0.Operations.Operation0.NumOperands = 1;
	csmd.State0.Operations.Operation0.Result.RegisterNumber = KukaLBR4PlusWorkingRegisters::CartesianPositionCommandRegister;
	csmd.State0.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;
	csmd.State0.Operations.Operation0.Operand0.RegisterNumber = KukaLBR4PlusFeedbackRegisters::MeasuredCartesianPositionRegister;
	csmd.State0.Operations.Operation0.Operand0.RegisterType = RegisterTypes::Feedback;

	// When the state of the FRI connection becomes CMD, transition to state 1
	// Value to compare FRI connection state to is stored in Scalar1
	csmd.State0.Transition0.NumTestOperands = 2;
	csmd.State0.Transition0.TestOpCode = KukaLBR4Operations::AreScalarsEqualOp;
	csmd.State0.Transition0.TestOperand0.RegisterNumber = KukaLBR4PlusFeedbackRegisters::FRIStateRegister;
	csmd.State0.Transition0.TestOperand0.RegisterType = RegisterTypes::Feedback;
	csmd.State0.Transition0.TestOperand1.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar1Register;
	csmd.State0.Transition0.TestOperand1.RegisterType = RegisterTypes::Working;
	csmd.State0.Transition0.TargetState = 1;
	csmd.State0.Transition0.Operations.NumOperations = 0;

	csmd.State1.ControlMode = ControlModes::CartesianImpedanceControlMode;
	csmd.State1.NumTransitions = 0;
	csmd.State1.Operations.NumOperations = 2;

	csmd.State1.Operations.Operation0.OpCode = KukaLBR4Operations::MultiplyFramesOp;
	csmd.State1.Operations.Operation0.NumOperands = 2;

	// Operation 0 multiplies the reported command frame by the FRI offset frame to get the current commanded position
	// and places that in the Frame2 register
	csmd.State1.Operations.Operation0.Result.RegisterNumber = KukaLBR4PlusWorkingRegisters::Frame2Register;
	csmd.State1.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;

	csmd.State1.Operations.Operation0.Operand0.RegisterNumber = KukaLBR4PlusFeedbackRegisters::CommandedCartesianPositionRegister;
	csmd.State1.Operations.Operation0.Operand0.RegisterType = RegisterTypes::Feedback;

	csmd.State1.Operations.Operation0.Operand1.RegisterNumber = KukaLBR4PlusFeedbackRegisters::CommandedCartesianOffsetRegister;
	csmd.State1.Operations.Operation0.Operand1.RegisterType = RegisterTypes::Feedback;

    // Operation 1 does linear Cartesian interpolation using the current commanded position in the Frame2
	// register as a starting point.
	csmd.State1.Operations.Operation1.OpCode = KukaLBR4Operations::SimpleCartesianInterpolationOp;
	csmd.State1.Operations.Operation1.NumOperands = 5;
	csmd.State1.Operations.Operation1.Result.RegisterNumber = KukaLBR4PlusWorkingRegisters::CartesianPositionCommandRegister;
	csmd.State1.Operations.Operation1.Result.RegisterType = RegisterTypes::Working;
	csmd.State1.Operations.Operation1.Operand0.RegisterNumber = KukaLBR4PlusWorkingRegisters::Frame2Register;
	csmd.State1.Operations.Operation1.Operand0.RegisterType = RegisterTypes::Working;

	// Desired Cartesian position is put in Frame1
	csmd.State1.Operations.Operation1.Operand1.RegisterNumber = KukaLBR4PlusWorkingRegisters::Frame1Register;
	csmd.State1.Operations.Operation1.Operand1.RegisterType = RegisterTypes::Working;

	// Max linear velocity in Scalar 2
	csmd.State1.Operations.Operation1.Operand2.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar2Register;
	csmd.State1.Operations.Operation1.Operand2.RegisterType = RegisterTypes::Working;

	// Max rotational velocity in Scalar 3
	csmd.State1.Operations.Operation1.Operand3.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar3Register;
	csmd.State1.Operations.Operation1.Operand3.RegisterType = RegisterTypes::Working;

	// Cycle time in ms in scalar 4
	csmd.State1.Operations.Operation1.Operand4.RegisterNumber = KukaLBR4PlusWorkingRegisters::Scalar4Register;
	csmd.State1.Operations.Operation1.Operand4.RegisterType = RegisterTypes::Working;

	return csmd;
}
